﻿#define HOMEBREW
#undef HOMEBREW

using System;
using System.Windows.Forms;
using System.IO;


namespace RobobuilderLib
{
    public partial class Main : Form
    {


        Preset_frm      presets = null;
        Video_frm       videoc  = null;
        MotionEdit_frm  medit   = null;
        Display3D_frm   view = null;
        balance_frm     bal = null;
        PCremote        pcR = null;

        wckMotion w;

        string robot_config = "config-20dof.txt";

        public Main()
        {
            InitializeComponent();

            presets = new Preset_frm();
            videoc = new Video_frm(presets);
            medit = new MotionEdit_frm();
            bal = new balance_frm();

            serialPort1.PortName = "COM5";
            serialPort1.BaudRate = 115200;
            serialPort1.ReadTimeout = 1000;
            serialPort1.WriteTimeout = 1000;

            listBox1.Items.Clear();
            foreach (string s in System.IO.Ports.SerialPort.GetPortNames())
            {
                listBox1.Items.Add(s);
            }

            label1.Text = "Disconnected";
            loadconfig();

        }

        void loadconfig()
        {
            try
            {
                string line;
                TextReader tr = new StreamReader("default.ini");
                while ((line = tr.ReadLine()) != null)
                {
                    line = line.Trim();
                    string[] nvp = line.Split('=');
                    switch (nvp[0])
                    {
                        case "BUTTONS":
                            presets.button_dir = nvp[1]; //
                            //presets.build_buttons();
                            break;
                        case "RCONFIG":
                            robot_config = nvp[1]; //
                            //presets.build_buttons();
                            break;
                        case "BASIC":
                            string bp = nvp[1]; // filename of basic pose
                            break;
                        case "DEBUG":
                            break;
                        case "DHMODE":
                            dhmode.Checked = true;
                            break;
                        case "COM":
                            serialPort1.PortName = nvp[1];
                            break;
                        case "VIDEO":
                            break;
                    }
                }
                tr.Close();
            }
            catch (Exception e1)
            {
                Console.WriteLine("?Can't open default.ini - " + e1.Message);
            }
            presets.presets_flg = false; // now exit will ask if any further changes
        }

        void saveconfig()
        {
            try
            {
                TextWriter tw = new StreamWriter("default.ini"); //overwrite if exists
                tw.WriteLine("#Autogenerated");
                tw.WriteLine("BUTTONS={0}", presets.button_dir);
                if (dhmode.Checked) tw.WriteLine("DHMODE");
                tw.Close();
            }
            catch (Exception e1)
            {
                Console.WriteLine("?Can't write to default.ini - " + e1.Message);
            }
        }


        /**********************************************
         * 
         * connect / disconnect serial port to RBC controller
         * 
         * ********************************************/

        private void connect_Click(object sender, EventArgs e)
        {
            if (serialPort1.IsOpen)
            {
                serialPort1.Close();
                connect.Text = "Connect";
                set_buttons(false);
                listBox1.Enabled = true;

                label1.Text = "Disconnected";
                pcR = null;

            }
            else
            {
                try
                {
                    serialPort1.Open();

                    if (pcR == null) pcR = new PCremote(serialPort1);
                }
                catch
                {
                    label1.Text = "Failed";
                    return;
                }

                // start up on connect

                w = new wckMotion(pcR);
                dcmp_cb.Checked = w.wckReadPos(30, 0);

                if (dcmp_cb.Checked) // DCMP  firmware
                {
                    label1.Text = String.Format("DCMP mode {0}.{1}", w.respnse[0], w.respnse[1]);
                    button6.Visible = false; button7.Visible = false; button9.Visible = false; button10.Visible = false;
                    button4.Visible = false; 

                    if (dhmode.Checked)
                        w.PlayPose(1000, 10, wckMotion.basicdh, true);
                }
                else
                {
                    w.close(); // assume standard firmware in default mode. 

                    string v = pcR.readVer();
                    if (v == "")
                    {
                        serialPort1.Close();
                        label1.Text = "Failed to connect";
                        textBox1.AppendText(pcR.message);
                        pcR = null;
                        return;
                    }

                    string sn = pcR.readSN();
                    label1.Text = "Firmware=" + v + ", S/N=" + sn;

                    // check Firmware - and if Homebrew enable Basic
                    if (sn != "" && sn.Substring(0, 2) == "HB")
                    {
                        toolStripMenuItem1.Visible = true;
                    }
                }

                connect.Text = "Close";
                listBox1.Enabled = false;
                set_buttons(true);

                textBox1.Text = "Connected on " + serialPort1.PortName;
            }
        }

        private void clr_Click(object sender, EventArgs e)
        {
            textBox1.Text = "";
        }

        private void set_buttons(bool f)
        {
            button1.Enabled = f;
            button2.Enabled = f;
            button3.Enabled = f;
            button4.Enabled = f; 
            button5.Enabled = f;
            button6.Enabled = f;
            button7.Enabled = f;
            button8.Enabled = f;
            button9.Enabled = f;
            button10.Enabled = f;
        }

        /**********************************************
         * 
         * Action buttons  - Remote / serial prorocol
         * 
         * ********************************************/

        private void button1_Click(object sender, EventArgs e)
        {
            if  (dcmp_cb.Checked)
                textBox1.AppendText("DCMP mode\r\n");
            else
                textBox1.AppendText("Ver=" + pcR.readVer() +"\r\n");
        }

        private void button2_Click(object sender, EventArgs e)
        {
            if (dcmp_cb.Checked)
                textBox1.AppendText("DCMP mode\r\n");
            else 
                textBox1.AppendText("S/N=" + pcR.readSN() + "\r\n");
        }

        private void button5_Click(object sender, EventArgs e)
        {
            // read distance

            if (dcmp_cb.Checked)
            {
                if (w.wckReadPos(30, 5))
                    textBox1.AppendText(String.Format("Distance={0}\r\n", w.respnse[0]));
                else
                    textBox1.AppendText("Fail\r\n");
                return;
            }

            textBox1.AppendText("Distance=" + pcR.readDistance() + "cm\r\n");
        }

        private void button6_Click(object sender, EventArgs e)
        {
            if (dcmp_cb.Checked)
                textBox1.AppendText("DCMP mode\r\n");
            else 
                textBox1.AppendText("Reset mem - " + pcR.resetMem());
        }

        private void button1_Click_1(object sender, EventArgs e)
        {
            //read XYZ

            if (dcmp_cb.Checked)
            {
                if (w.wckReadAll())
                    textBox1.AppendText(String.Format("XYZ={0},{1},{2}\r\n", w.cbyte(w.respnse[0]), w.cbyte(w.respnse[1]), w.cbyte(w.respnse[2])));
                else
                    textBox1.AppendText("Fail\r\n");
                return;
            }
            int x,y,z;
            textBox1.AppendText(pcR.readXYZ(out x, out y, out z) + "\r\n");
        }
        
        private void button7_Click(object sender, EventArgs e)
        {
            // avail mem
            if (dcmp_cb.Checked)
                textBox1.AppendText("DCMP mode\r\n");
            else
                textBox1.AppendText(pcR.availMem() + "\r\n");
        }

        private void button9_Click(object sender, EventArgs e)
        {
            //read zeros
            if (dcmp_cb.Checked)
                textBox1.AppendText("DCMP mode\r\n");
            else
                textBox1.AppendText(pcR.readZeros() + "\r\n");
        }

        private void button2_Click_1(object sender, EventArgs e)
        {
            if (dcmp_cb.Checked)
            {
                if (w.wckReadPos(30, 7))
                    textBox1.AppendText(String.Format("IR={0} {1}\r\n", w.respnse[0],w.respnse[1]  ));
                else
                    textBox1.AppendText("Fail\r\n");
                return;
            }
            // read IR - not working
            button2.Enabled = false;
            pcR.readIR(10000, new callBack(addMessage));
            button2.Enabled = true;
        }

        private void button4_Click(object sender, EventArgs e)
        {
            // read Button - 
            button4.Enabled = false;
            pcR.readButton(10000, new callBack(addMessage));
            button4.Enabled = true;
        }

        private void button8_Click(object sender, EventArgs e)
        {
            // read Soundlevel - 
            if (dcmp_cb.Checked)
            {
                if (w.wckReadAll())
                    textBox1.AppendText(String.Format("Sound={0}\r\n", w.respnse[6]));
                else
                    textBox1.AppendText("Fail\r\n");
                return;
            }
            button8.Enabled = false;
            pcR.readsoundLevel(10000, Convert.ToInt32(textBox2.Text), new callBack(addMessage));
            button8.Enabled = true;
        }

        private void button3_Click(object sender, EventArgs e)
        {
            //read any
            if (dcmp_cb.Checked)
            {
                if (w.wckReadAll())
                    textBox1.AppendText(String.Format("Any={0}\r\n", w.Message));
                else
                    textBox1.AppendText("Fail\r\n");


                return;
            }

            // wait for IR, Button or Sound
            button3.Enabled = false;

            pcR.readsoundLevel(10000, Convert.ToInt32(textBox2.Text), new callBack(addMessage));
            pcR.readButton(10000, new callBack(addMessage));
            pcR.readIR(10000, new callBack(addMessage));

            button3.Enabled = true;
        }


        public void addMessage(int n)
        {
            if (n!=0)
                textBox1.AppendText("value received = " + n + "\r\n");
            else
                textBox1.AppendText(pcR.message + "\r\n");
        }

        private void button10_Click(object sender, EventArgs e)
        {
            textBox1.AppendText(pcR.zeroHuno() + "\r\n");
        }

        /**********************************************
         * 
         * Event routines
         * 
         * change of serial port
         * 
         * ********************************************/

        private void listBox1_SelectedIndexChanged(object sender, EventArgs e)
        {
            if (!serialPort1.IsOpen)
                serialPort1.PortName = listBox1.Items[listBox1.SelectedIndex].ToString();
        }

        private void s0_Click(object sender, EventArgs e)
        {
            Console.WriteLine(((Label)sender).Text);
        }

        private void presetsToolStripMenuItem_Click(object sender, EventArgs e)
        {
            presets.connect(pcR);
        }

        private void videoToolStripMenuItem_Click(object sender, EventArgs e)
        {
            videoc.Show();
        }

        private void motionEditToolStripMenuItem_Click(object sender, EventArgs e)
        {
            medit.DCMP = dcmp_cb.Checked;
            medit.dhf = dhmode.Checked;

            medit.connect(pcR);
        }

        private void exitToolStripMenuItem_Click(object sender, EventArgs e)
        {
            if (presets.presets_flg == false)
            {
                this.Close();
            }

            switch (MessageBox.Show("Update default.ini?", "exit", MessageBoxButtons.YesNoCancel))
            {
                case DialogResult.Yes:
                    saveconfig();
                    this.Close();
                    break;
                case DialogResult.No:
                    this.Close();
                    break;
                case DialogResult.Cancel:
                    break;
            }
        }

        private void viewModelToolStripMenuItem_Click(object sender, EventArgs e)
        {
            if (view == null) view = new Display3D_frm(robot_config);
            view.Show();
            medit.viewport = view;

            while (view.Created)
            {
                view.render();
                Application.DoEvents();
            }
            view = null;
            medit.viewport = null;
        }

        private void balance_Click(object sender, EventArgs e)
        {
            bal.pcr = pcR;
            bal.Show();
        }

        private void toolStripMenuItem1_Click(object sender, EventArgs e)
        {
            this.Close();
        }

        //private void Main_Deactivate(object sender, EventArgs e)
        //{
        //    this.Close();
        //}


    }
}
